﻿using System.Collections.Generic;
using Microsoft.Xna.Framework;

namespace Cubezicles
{
    public class Graph
    {
        public List<Node> nodes = new List<Node>();
        public List<Edge> edges = new List<Edge>();

        public Graph() { }

        public void addNode(Node n) 
        {
            nodes.Add(n);
            n.id = nodes.IndexOf(n);
        }
        public void addNode(Vector3 position)
        {
            Node n = new Node(position);
            nodes.Add(n);
            n.id = nodes.IndexOf(n);
        }

        public void addEdge(Edge e)
        {
            edges.Add(e);
        }

        public void addEdge(Node n1, Node n2) 
        {
            edges.Add(new Edge(n1, n2));
        }
        
        public List<Node> getPath(Node n1, Node n2) {
            Dijkstra djik = new Dijkstra(edges, nodes);
            djik.calculateDistance(n1);
            return djik.getPathTo(n2);
        }
    }

    //Adapted from http://www.hann3mann.de/artikel/dijkstra-algorithmus-in-c-csharp/
    public class Dijkstra
    {
        private List<Node> Nodes;
        private List<Edge> Edges;
        private List<Node> Basis;
        private Dictionary<int, double> Dist;
        private Dictionary<int, Node> Previous;

        /// <summary>
        /// Konstruktor Don't question why it's in German. Just enjoy it. 
        /// </summary>
        /// <param name="edges">Liste aller Kanten</param>
        /// <param name="nodes">Liste aller Knoten</param>
        public Dijkstra(List<Edge> edges, List<Node> nodes)
        {
            Edges = edges;
            Nodes = nodes;
            Basis = new List<Node>();
            Dist = new Dictionary<int, double>();
            Previous = new Dictionary<int, Node>();

            // Knoten aufnehmen
            foreach (Node n in Nodes)
            {
                Previous.Add(n.id, null);
                Basis.Add(n);
                Dist.Add(n.id, double.MaxValue);
            }
        }

        /// <summary>
        /// Berechnet die kürzesten Wege vom start
        /// Knoten zu allen anderen Knoten
        /// </summary>
        /// <param name="start">Startknoten</param>
        public void calculateDistance(Node start)
        {
            Dist[start.id] = 0;

            while (Basis.Count > 0)
            {
                Node u = getNodeWithSmallestDistance();
                if (u == null)
                {
                    Basis.Clear();
                }
                else
                {
                    foreach (Node v in getNeighbors(u))
                    {
                        double alt = Dist[u.id] +
                                getDistanceBetween(u, v);
                        if (alt < Dist[v.id])
                        {
                            Dist[v.id] = alt;
                            Previous[v.id] = u;
                        }
                    }
                    Basis.Remove(u);
                }
            }
        }

        /// <summary>
        /// Liefert den Pfad zum Knoten d
        /// </summary>
        /// <param name="d">Zielknote<n/param>
        /// <returns></returns>
        public List<Node> getPathTo(Node d)
        {
            List<Node> path = new List<Node>();

            path.Insert(0, d);

            while (Previous[d.id] != null)
            {
                d = Previous[d.id];
                path.Insert(0, d);
            }

            return path;
        }

        /// <summary>
        /// Liefert den Knoten mit der kürzesten Distanz
        /// </summary>
        /// <returns></returns>
        public Node getNodeWithSmallestDistance()
        {
            double distance = double.MaxValue;
            Node smallest = null;

            foreach (Node n in Basis)
            {
                if (Dist[n.id] < distance)
                {
                    distance = Dist[n.id];
                    smallest = n;
                }
            }

            return smallest;
        }

        /// <summary>
        /// Liefert alle Nachbarn von n die noch in der Basis sind
        /// </summary>
        /// <param name="n">Knoten</param>
        /// <returns></returns>
        public List<Node> getNeighbors(Node n)
        {
            List<Node> neighbors = new List<Node>();

            foreach (Edge e in Edges)
            {
                if (e.n1.Equals(n) && Basis.Contains(n))
                {
                    neighbors.Add(e.n2);
                }
                else if (e.n2.Equals(n) && Basis.Contains(n))
                {
                    neighbors.Add(e.n1);
                }
            }

            return neighbors;
        }

        /// <summary>
        /// Liefert die Distanz zwischen zwei Knoten
        /// </summary>
        /// <param name="o">Startknoten</param>
        /// <param name="d">Endknoten</param>
        /// <returns></returns>
        public double getDistanceBetween(Node o, Node d)
        {
            foreach (Edge e in Edges)
            {
                if (e.n1.Equals(o) && e.n2.Equals(d))
                {
                    return e.distance;
                }
            }

            return 0;
        }
    }

    public class Node
    {
        public Vector3 pos;
        public int id = -1;
        private readonly static string format = "ID: {0}  X: {1}  Y: {2}  Z: {3}";

        public Node(Vector3 p)
        {
            pos = p;
        }

        public new string ToString()
        {
            return string.Format(format, id, pos.X, pos.Y, pos.Z);
        }
    }

    public class Edge
    {
        public Node n1, n2;
        public double distance;

        public Edge(Node node1, Node node2)
        {
            n1 = node1;
            n2 = node2;
            distance = getDistance(n1, n2);
        }

        public static double getDistance(Node n1, Node n2)
        {
            return Vector3.Distance(n1.pos, n2.pos);
        }

        public static bool closeEnough(Node n1, Node n2)
        {
            return getDistance(n1, n2) < 2;
        }
    }
}
